In [1]:
import os
import numpy as np
import cv2
import json 

# ------------------------------
# ENTER YOUR REQUIREMENTS HERE:
ARUCO_DICT = cv2.aruco.DICT_4X4_50
SQUARES_VERTICALLY = 5
SQUARES_HORIZONTALLY = 7
#SQUARE_LENGTH = 0.03  # in meters
#MARKER_LENGTH = 0.015  # in meters
SQUARE_LENGTH = 0.10 # in meters
MARKER_LENGTH = 0.08  # in meters
# ...
PATH_TO_YOUR_IMAGES = 'testdata/charuco_calib_set_1/'
print(f"{ARUCO_DICT=}")
# ------------------------------

json_file_path = './calibration.json'

with open(json_file_path, 'r') as file: # Read the JSON file
    json_data = json.load(file)

mtx = np.array(json_data['mtx'])
dst = np.array(json_data['dist'])

dictionary = cv2.aruco.getPredefinedDictionary(ARUCO_DICT)
board = cv2.aruco.CharucoBoard((SQUARES_VERTICALLY, SQUARES_HORIZONTALLY), SQUARE_LENGTH, MARKER_LENGTH, dictionary)
params = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(dictionary, params)

ARUCO_DICT=0
In [2]:
image_paths = [os.path.join(PATH_TO_YOUR_IMAGES, f) for f in os.listdir(PATH_TO_YOUR_IMAGES) if f.endswith(".JPG")]

img_path = image_paths[10] # select image
image_color = cv2.imread(img_path)
image = cv2.cvtColor(image_color, cv2.COLOR_BGR2GRAY)

h,  w = image.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dst, (w,h), 1, (w,h))
image = cv2.undistort(image, mtx, dst, None, newcameramtx)

all_charuco_ids = []
all_charuco_corners = []


marker_corners, marker_ids, rejectedCandidates = detector.detectMarkers(image)
if marker_ids is not None and len(marker_ids) > 0: # If at least one marker is detected
    # cv2.aruco.drawDetectedMarkers(image_copy, marker_corners, marker_ids)
    ret, charucoCorners, charucoIds = cv2.aruco.interpolateCornersCharuco(marker_corners, marker_ids, image, board)

    retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard(charucoCorners, charucoIds, board, np.array(mtx), np.array(dst), np.empty(1), np.empty(1))

    Zx, Zy, Zz = tvec[0][0], tvec[1][0], tvec[2][0]
    fx, fy = mtx[0][0], mtx[1][1]

else:
    print("no marker detected")
In [3]:
%matplotlib widget
In [4]:
import matplotlib.pyplot as plt
plt.figure()
plt.imshow(cv2.cvtColor(image_color, cv2.COLOR_BGR2RGB))
plt.scatter(np.hstack(marker_corners)[0,:,0],np.hstack(marker_corners)[0,:,1],label="marker Corners")
plt.scatter(charucoCorners[:,0,0],charucoCorners[:,0,1], label="charuco Corners")
plt.legend()
Out[4]:
<matplotlib.legend.Legend at 0x7f9a9043f3e0>
Figure
No description has been provided for this image
In [5]:
rotation_matrix, _ = cv2.Rodrigues(rvec)

# Camera pose: the inverse of the extrinsic matrix (rotation + translation)
camera_pose = np.eye(4)
camera_pose[:3, :3] = rotation_matrix
camera_pose[:3, 3] = tvec.flatten()
camera_mat = np.eye(4)
camera_mat[:3,:3] = mtx
P = camera_mat @ camera_pose
P = P[:3,:] # we only had 4 rows for multiplication before
world_corners = board.getChessboardCorners()
image_pts = P @ np.hstack((world_corners,np.ones((world_corners.shape[0],1)))).T
# recover homogeneous coordinates
image_pts = image_pts / image_pts[-1,:]
image_pts = image_pts[:2,:]
image_pts.T
Out[5]:
array([[3496.29498882, 2637.96890187],
       [3404.36141996, 2147.46211359],
       [3315.40360475, 1672.83230461],
       [3229.27938211, 1213.32098179],
       [3820.67235209, 2546.00032368],
       [3731.34049899, 2096.91181377],
       [3644.66772727, 1661.19102043],
       [3560.53706993, 1238.24992796],
       [4095.40790656, 2468.10636071],
       [4008.97857633, 2053.98953234],
       [3924.930495  , 1651.28220165],
       [3843.16660774, 1259.51934085],
       [4331.08717634, 2401.285769  ],
       [4247.6603771 , 2017.08981195],
       [4166.37291348, 1642.74589413],
       [4087.14355537, 1277.8799357 ],
       [4535.48583493, 2343.33396303],
       [4455.04753566, 1985.02817953],
       [4376.53833326, 1635.31539926],
       [4299.88967062, 1293.89024021],
       [4714.44396671, 2292.59514214],
       [4636.9162088 , 1956.91165299],
       [4561.13464298, 1628.788912  ],
       [4487.04094723, 1307.97439364]])
In [6]:
import matplotlib.pyplot as plt
plt.figure()
plt.imshow(cv2.cvtColor(image_color, cv2.COLOR_BGR2RGB))
plt.scatter(np.hstack(marker_corners)[0,:,0],np.hstack(marker_corners)[0,:,1],label="marker Corners")
plt.scatter(charucoCorners[:,0,0],charucoCorners[:,0,1], label="charuco Corners")
plt.scatter(image_pts[0,:],image_pts[1,:], label="reprojected Corners")
plt.legend()
Out[6]:
<matplotlib.legend.Legend at 0x7f9a8e1fbb00>
Figure
No description has been provided for this image
In [7]:
ic = cv2.drawFrameAxes(image_color.copy(), mtx,dst, rvec, tvec, 0.25, 25)
plt.figure()
plt.imshow(ic)
plt.show()
Figure
No description has been provided for this image

successfully overlaid corners mean that returned rvec and tvec are indeed the camera pose. Note that because of opencv the image origin is top left

In [8]:
image_pts
Out[8]:
array([[3496.29498882, 3404.36141996, 3315.40360475, 3229.27938211,
        3820.67235209, 3731.34049899, 3644.66772727, 3560.53706993,
        4095.40790656, 4008.97857633, 3924.930495  , 3843.16660774,
        4331.08717634, 4247.6603771 , 4166.37291348, 4087.14355537,
        4535.48583493, 4455.04753566, 4376.53833326, 4299.88967062,
        4714.44396671, 4636.9162088 , 4561.13464298, 4487.04094723],
       [2637.96890187, 2147.46211359, 1672.83230461, 1213.32098179,
        2546.00032368, 2096.91181377, 1661.19102043, 1238.24992796,
        2468.10636071, 2053.98953234, 1651.28220165, 1259.51934085,
        2401.285769  , 2017.08981195, 1642.74589413, 1277.8799357 ,
        2343.33396303, 1985.02817953, 1635.31539926, 1293.89024021,
        2292.59514214, 1956.91165299, 1628.788912  , 1307.97439364]])
In [ ]:
In [9]:
camera_pose
Out[9]:
array([[-0.16105014,  0.89798268, -0.40949965,  0.11415069],
       [-0.98380074, -0.11296621,  0.13919315,  0.20722587],
       [ 0.07873341,  0.42528313,  0.90162925,  0.42033198],
       [ 0.        ,  0.        ,  0.        ,  1.        ]])
In [ ]:
In [ ]:
In [ ]:
In [10]:
import plotly.graph_objects as go
from stl import mesh
def plot_3d_points_and_camera_centers(points, extrinsic_matrices):
    """
    Plots a 3D scatter plot of the given points and camera representations from extrinsic matrices.

    Parameters:
    - points: A 3xN NumPy array, where each column represents a point (x, y, z).
    - extrinsic_matrices: A list of 4x4 NumPy arrays representing the extrinsic matrices for each camera.
    """
    # Ensure points is a NumPy array
    points = np.asarray(points)

    # Check if the input is a 3xN array
    if points.shape[0] != 3:
        raise ValueError("Input points must be a 3xN NumPy array.")

    # Extract x, y, z coordinates
    x = points[0, :]
    y = points[1, :]
    z = points[2, :]

    # Create a 3D scatter plot for the points
    fig = go.Figure(data=[go.Scatter3d(
        x=x,
        y=y,
        z=z,
        mode='markers',
        marker=dict(
            size=5,
            color='blue',  # Marker color for points
            opacity=0.8
        ),
        name='3D Points'
    )])

    # Function to create a cone shape for the camera representation
    def create_camera_cone(extrinsic):
        # Transform the cone points using the extrinsic matrix
        camera_center = extrinsic[:3, 3]
        rotation_matrix = extrinsic[:3, :3]


        def rotate_and_translate(vertices, rotation_matrix, translation_vector):
            # Apply rotation
            rotated_vertices = vertices @ rotation_matrix.T  # Ensure correct shape
            # Apply translation
            translated_vertices = rotated_vertices + translation_vector
            return translated_vertices

        # Load the STL file
        your_mesh = mesh.Mesh.from_file('pyramid.stl')

        # Extract vertices
        vertices = your_mesh.vectors.reshape(-1, 3)

        # Rotate and translate the frustum
        transformed_vertices = rotate_and_translate(vertices, rotation_matrix, camera_center)
        # compute forward vector 
        frustum = go.Mesh3d(
            x=transformed_vertices[:, 0],
            y=transformed_vertices[:, 1],
            z=transformed_vertices[:, 2],
            opacity=0.75,
            color='blue'
        )

        return frustum

    # Plot camera representations from extrinsic matrices
    for idx, extrinsic in enumerate(extrinsic_matrices):
        # Create the camera cone
        cone_points = create_camera_cone(extrinsic)

        # Add the cone to the plot
        fig.add_trace(cone_points)

    # Set the layout
    fig.update_layout(
        title='3D Scatter Plot of Points and Camera Representations',
        scene=dict(
            xaxis_title='X Axis',
            yaxis_title='Y Axis',
            zaxis_title='Z Axis'
        )
    )

    # Show the figure in the Jupyter Notebook
    fig.show()


# Define a list of extrinsic matrices (4x4)
extrinsic_matrices = [
    camera_pose
]

# Call the function to plot the points and camera representations
plot_3d_points_and_camera_centers(world_corners.T, extrinsic_matrices)
In [11]:
camera_pose
Out[11]:
array([[-0.16105014,  0.89798268, -0.40949965,  0.11415069],
       [-0.98380074, -0.11296621,  0.13919315,  0.20722587],
       [ 0.07873341,  0.42528313,  0.90162925,  0.42033198],
       [ 0.        ,  0.        ,  0.        ,  1.        ]])

now use validated code to estimate position of all cameras:

In [12]:
def reproject_world_points(rvec, tvec, mtx, world_corners):
    rotation_matrix, _ = cv2.Rodrigues(rvec)

    # Camera pose: the inverse of the extrinsic matrix (rotation + translation)
    camera_pose = np.eye(4)
    camera_pose[:3, :3] = rotation_matrix
    camera_pose[:3, 3] = tvec.flatten()
    camera_mat = np.eye(4)
    camera_mat[:3,:3] = mtx
    P = camera_mat @ camera_pose
    P = P[:3,:] # we only had 4 rows for multiplication before
    image_pts = P @ np.hstack((world_corners,np.ones((world_corners.shape[0],1)))).T
    # recover homogeneous coordinates
    image_pts = image_pts / image_pts[-1,:]
    image_pts = image_pts[:2,:]
    return camera_pose, P, image_pts

def estimate_pose(image_color, mtx, dst, detector):
    image = cv2.cvtColor(image_color, cv2.COLOR_BGR2GRAY)

    h,  w = image.shape[:2]
    newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dst, (w,h), 1, (w,h))
    image = cv2.undistort(image, mtx, dst, None, newcameramtx)

    all_charuco_ids = []
    all_charuco_corners = []


    marker_corners, marker_ids, rejectedCandidates = detector.detectMarkers(image)
    if marker_ids is not None and len(marker_ids) > 0: # If at least one marker is detected
        # cv2.aruco.drawDetectedMarkers(image_copy, marker_corners, marker_ids)
        ret, charucoCorners, charucoIds = cv2.aruco.interpolateCornersCharuco(marker_corners, marker_ids, image, board)

        retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard(charucoCorners, charucoIds, board, np.array(mtx), np.array(dst), np.empty(1), np.empty(1))

        Zx, Zy, Zz = tvec[0][0], tvec[1][0], tvec[2][0]
        fx, fy = mtx[0][0], mtx[1][1]
        if retval == 0: 
            return None
        return rvec, tvec, charucoCorners
    else:
        return None
        #print("no marker detected")
In [13]:
from scipy.spatial import cKDTree
def compute_reprojection_error(reprojected_points, detected_points):
    # Create a KDTree for fast nearest neighbor search
    tree = cKDTree(detected_points)

    # Find the nearest detected point for each reprojected point
    distances, indices = tree.query(reprojected_points)

    # Calculate the reprojection error as the mean distance
    reprojection_error = np.mean(distances)

    return reprojection_error
In [14]:
def invert_transform(rotvec, tvec):
    # Step 1: Convert rotation vector to rotation matrix
    R, _ = cv2.Rodrigues(rotvec)

    # Step 2: Compute the inverse of the rotation matrix
    R_inv = R.T

    # Step 3: Convert the inverse rotation matrix back to a rotation vector
    rotvec_inv, _ = cv2.Rodrigues(R_inv)

    # Step 4: Compute the inverse translation vector
    tvec_inv = -R_inv @ tvec
    return rotvec_inv, tvec_inv

def draw_reproj(image_color, charucoCorners, image_pts):
    f = plt.figure()
    ax = f.add_subplot(111)
    ax.imshow(cv2.cvtColor(image_color, cv2.COLOR_BGR2RGB))
    #plt.scatter(np.hstack(marker_corners)[0,:,0],np.hstack(marker_corners)[0,:,1],label="marker Corners")
    ax.scatter(charucoCorners[:,0,0],charucoCorners[:,0,1], label="charuco Corners")
    ax.scatter(image_pts[0,:],image_pts[1,:], label="reprojected Corners")
    ax.legend()
    plt.close(f)
    return f

def convert_left_to_right(rvec, tvec):
    # Convert rotation vector to rotation matrix
    R, _ = cv2.Rodrigues(rvec)

    # Invert the z-axis for the rotation matrix
    R[:, 2] = -R[:, 2]

    # Convert back to rotation vector
    rvec_right = cv2.Rodrigues(R)[0]

    # Invert the z-component of the translation vector
    tvec_right = tvec.copy()
    tvec_right[2] = -tvec_right[2]

    return rvec_right, tvec_right
In [15]:
REPROJECTION_ERROR_THRESH = 10000.0
dictionary = cv2.aruco.getPredefinedDictionary(ARUCO_DICT)
board = cv2.aruco.CharucoBoard((SQUARES_VERTICALLY, SQUARES_HORIZONTALLY), SQUARE_LENGTH, MARKER_LENGTH, dictionary)
params = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(dictionary, params)
world_corners = board.getChessboardCorners()

camera_poses = []
figures = []
for i, image_path in enumerate(image_paths):
    #img_path = image_paths[12] # select image
    #print(image_path)
    image_color = cv2.imread(image_path)
    res = estimate_pose(image_color, mtx, dst, detector)
    if res == None:
        print("pose could not be estimated")
    rvec, tvec, charuco_corners = res
    #rvec, tvec = convert_left_to_right(rvec, tvec)
    #rvec, tvec = invert_transform(rvec, tvec)
    E, P, img_pts = reproject_world_points(rvec, tvec, mtx, world_corners)
    #print(charuco_corners[:2,0,:])
    repr_err = compute_reprojection_error(charuco_corners, img_pts.T)
    if repr_err < REPROJECTION_ERROR_THRESH:
        camera_poses.append(E)
        figures.append(draw_reproj(image_color, charuco_corners, img_pts))
    print(i, image_path,repr_err)


plot_3d_points_and_camera_centers(world_corners.T, camera_poses)

0 testdata/charuco_calib_set_1/GOPR0016.JPG 2.4047658584014737
1 testdata/charuco_calib_set_1/GOPR0017.JPG 1.1539104109021663
2 testdata/charuco_calib_set_1/GOPR0018.JPG 1.5094955400676695
3 testdata/charuco_calib_set_1/GOPR0019.JPG 2.9881964206864953
4 testdata/charuco_calib_set_1/GOPR0020.JPG 4.304995267616654
5 testdata/charuco_calib_set_1/GOPR0021.JPG 3.0850779752409814
6 testdata/charuco_calib_set_1/GOPR0022.JPG 2.330988109181218
7 testdata/charuco_calib_set_1/GOPR0023.JPG 3.756626638282929
8 testdata/charuco_calib_set_1/GOPR0024.JPG 4.851068595352484
9 testdata/charuco_calib_set_1/GOPR0025.JPG 3.510577565476059
10 testdata/charuco_calib_set_1/GOPR0026.JPG 5.559737925918886
11 testdata/charuco_calib_set_1/GOPR0027.JPG 7.608089775930453
12 testdata/charuco_calib_set_1/GOPR0028.JPG 1.6177716689985049
13 testdata/charuco_calib_set_1/GOPR0029.JPG 2.784991333966658

note that no and especially not trace 11 (indexed by 11) is located in a way that could explain image GOPR0026. Tt should be next to the board but it is displayed above.

In [16]:
#camera_poses[10:12]
figures[10]
Out[16]:
No description has been provided for this image
In [ ]: